#include "MonitorTask.h"

OFFLINE_COUNT_STRUCT 			Fps;
OFFLINE_COUNT_STRUCT      startFps;
OFFLINE_COUNT_STRUCT      calFps;
void StartMonitorTask(void const * argument)
{
  /* USER CODE BEGIN StartMonitorTask */
  /* Infinite loop */
  for(;;)
  {
    static TickType_t xLastWakeTime;
    for(;;)
    {
      startFps.chassis[0] = Fps.chassis[0];
      startFps.chassis[1] = Fps.chassis[1];
      startFps.chassis[2] = Fps.chassis[2];
      startFps.chassis[3] = Fps.chassis[3];
      startFps.yaw =        Fps.yaw;
      startFps.pitch_r =    Fps.pitch_r;
      startFps.pitch_l =    Fps.pitch_l;
      startFps.trans =      Fps.trans;
      startFps.fric_l = Fps.fric_l;
      startFps.fric_r = Fps.fric_r;
      startFps.pc  =  Fps.pc;
      startFps.judge = Fps.judge;

      xLastWakeTime = xTaskGetTickCount();
      vTaskDelayUntil(&xLastWakeTime, 1000);

      calFps.chassis[0] = Fps.chassis[0] - startFps.chassis[0];
      calFps.chassis[1] = Fps.chassis[1] - startFps.chassis[1];
      calFps.chassis[2] = Fps.chassis[2] - startFps.chassis[2];
      calFps.chassis[3] = Fps.chassis[3] - startFps.chassis[3];
      calFps.yaw =        Fps.yaw - startFps.yaw;
      calFps.pitch_r =    Fps.pitch_r - startFps.pitch_r;
      calFps.pitch_l =    Fps.pitch_l - startFps.pitch_l;
      calFps.trans =      Fps.trans - startFps.trans;
      calFps.fric_l =     Fps.fric_l - startFps.fric_l;
      calFps.fric_r =     Fps.fric_r - startFps.fric_r;
      calFps.pc     =     Fps.pc - startFps.pc;
      calFps.judge= Fps.judge - startFps.judge;
    }
  }
  /* USER CODE END StartMonitorTask */
}

uint8_t isProcessOn(void)
{
	if(calFps.chassis[0] > 990 || calFps.chassis[1] > 990 ||\
		  calFps.chassis[2] > 990 || calFps.chassis[3] >990 )
			return 1;
	else 
		return 0;
}